using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;
using System.Threading;

namespace Autobot
{

    public enum VirtualRobotState : int
    {
        STARTUP=0,
        PLACE_ROBOT,
        RUN,
        STOP,
        NUM_STATES
    }

    /// <summary>
    /// This is the main type for your game
    /// </summary>
    public class VirtualRobot : Microsoft.Xna.Framework.Game
    {
        //Graphics device
        GraphicsDeviceManager graphics;

        //Remember the state of the program
        private VirtualRobotState currentState;

        //A string to write to the window
        //1st line of instructions, second line of instructions
        // and the state strings
        ShowString instructionsString1;
        ShowString instructionsString2;

        //Need to have a map and a filename to save to
        private VirtualMap map;
        private Robot robot;
        private string filename;
        private string communicator_ip_string = null;

        private CommunicatorClient communicator;
        private LogController communicator_log;

        //Information about the last state of keyboard and mouse
        private KeyboardState lastKeyboardInput;
        private MouseState lastMouseInput;

        private DateTime oldTime;
        
        private List<DelayedMessage> delayedMessages;

        private Thread delayedMessagesThread;

        private DateTime updateServerTime;
        private bool receiveServerMessages;
        private bool requestAlgorithmUpdate;
        private int parsedServerMessages;
        private int expectedServerMessages;


        #region Constructors
        // Constructors: Allow for different starting parameters
        // Default parameters-- windowSize: 800x600  fileName: map.map
        private Vector2 DefaultWindowSize = new Vector2(800, 600); // Default window size
        private const string DefaultFileName = "map.map"; // Default map name
        // Passed no arguments, use default file name and window size
        public VirtualRobot()
        {
            InitSimulator(DefaultWindowSize, DefaultFileName);
        }
        // Passed a single value, got file name use default window size
        public VirtualRobot(string fileName)
        {
            InitSimulator(DefaultWindowSize, fileName);
        }
        // Passed two values (2D vector), got window size use default map name
        public VirtualRobot(Vector2 windowSize)
        {
            InitSimulator(windowSize, DefaultFileName);
        }
        // Passed three values, use all three values for window size and file name
        public VirtualRobot(Vector2 windowSize, string fileName)
        {
            InitSimulator(windowSize, fileName);
        }
        // Passed four values, additional constructor
        public VirtualRobot(Vector2 windowSize, string fileName, string ip)
        {
            communicator_ip_string = ip;
            InitSimulator(windowSize, fileName);
        }
        #endregion

        // Initialize the simulator
        private void InitSimulator(Vector2 windowSize, string _fileName)
        {
            // Load up the graphics manager
            graphics = new GraphicsDeviceManager(this);

            // Set the window size according to input recieved, leave some room for instructions in the top 20 pixels of screen
            graphics.PreferredBackBufferHeight = (int)windowSize.Y + 70;
            graphics.PreferredBackBufferWidth = (int)windowSize.X;
            Window.Title = "Robot Simulator";

            //Set the directory for the content
            Content.RootDirectory = "Content";

            //Remember the name of the map file
            filename = _fileName;
                                   
            //The map will consume most of the window, remember to leave room for the instructions
            Rectangle MapRect = new Rectangle(0, 70, (int)windowSize.X, (int)windowSize.Y);

            //This is the first instructions that will be displayed to screen in the top left corner
            instructionsString1 = new ShowString(new Vector2(0f, 0f), "Hit -P To place the robot", 20, Color.Red);
            instructionsString2 = new ShowString(new Vector2(0f, 30f), "", 20, Color.Red);

            //Create a new map based on the input
            map = new VirtualMap(filename,MapRect);

            //Set up a new robot to start
            robot = new Robot(new Vector2(MapRect.Left + 10f, MapRect.Top + 10f), Math.PI/2.0);

            // Create a new logfile for the communicator
            communicator_log = new LogController();
            bool success = communicator_log.OpenLog("ClientLog.txt");
            if (!success)
            {
                Console.WriteLine("ERROR: could not open ClientLog.txt for writing");
                this.Exit();
            }

            //Set up the communicator
            communicator = new CommunicatorClient();

            // Initialize it, quit if it doesn't succeed
            if (!communicator.Initialize(communicator_log, communicator_ip_string))
                this.Exit();

            //Set the current state of the Editor to IDLE
            currentState = VirtualRobotState.STARTUP;
            oldTime = new DateTime();

            // Initialize the list for all delayed commands
            delayedMessages = new List<DelayedMessage>();

            delayedMessagesThread = new Thread(new ThreadStart(this.DelayedMessageThread));
            delayedMessagesThread.Start();

            receiveServerMessages = false;
            requestAlgorithmUpdate = false;
            parsedServerMessages = 0;
            expectedServerMessages = 0;
        }

        /// <summary>
        /// Allows the game to perform any initialization it needs to before starting to run.
        /// This is where it can query for any required services and load any non-graphic
        /// related content.  Calling base.Initialize will enumerate through any components
        /// and initialize them as well.
        /// </summary>
        protected override void Initialize()
        {
            // Initialize the states to the current keyboard/mouse states
            lastKeyboardInput = Keyboard.GetState();
            lastMouseInput = Mouse.GetState();

            // Make the mouse visible
            this.IsMouseVisible = true;
            base.Initialize();
        }

        /// <summary>
        /// LoadContent will be called once per game and is the place to load
        /// all of your content.
        /// </summary>
        protected override void LoadContent()
        {
            //Load map and robot content
            map.LoadContent(Content, GraphicsDevice);
            robot.LoadContent(Content, GraphicsDevice);

            //Load the string content
            instructionsString1.LoadContent(Content, GraphicsDevice);
            instructionsString2.LoadContent(Content, GraphicsDevice);
        }

        /// <summary>
        /// UnloadContent will be called once per game and is the place to unload
        /// all content.
        /// </summary>
        protected override void UnloadContent()
        {
            //Dispose of all graphical resources
            map.Dispose();
            robot.Dispose();
            instructionsString1.Dispose();
            instructionsString2.Dispose();

            communicator.Close();
            delayedMessagesThread.Abort();
        }

        /// <summary>
        /// Allows the game to run logic such as updating the world,
        /// checking for collisions, gathering input, and playing audio.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Update(GameTime gameTime)
        {
            //Check the current keyboard state
            KeyboardState keyboardInput = Keyboard.GetState();
            MouseState mouseInput = Mouse.GetState();

            //Check if we want to quit
            if (keyboardInput.IsKeyDown(Keys.Escape))
                this.Exit();

            //Enter the large state machine. eep.
            switch ((int)currentState)
            {
                case (int)VirtualRobotState.STARTUP:
                    #region Start up
                    // Set the show string and move straight to the next state
                    currentState = VirtualRobotState.PLACE_ROBOT;
                    instructionsString1.SetString("Hold down the left mouse button to drag the car");
                    #endregion
                    break;

                case (int)VirtualRobotState.PLACE_ROBOT:
                    #region Place Robot
                    // If the window isn't active, just break
                    if (!IsActive)
                        break;
                    #region Set Robot Position
                    //If the mouse button is down, let the user place the robot. dragging requires the state
                    //to be pressed, regardless of the previous state
                    if (mouseInput.LeftButton == ButtonState.Pressed && mouseInput.RightButton == ButtonState.Released)
                    {
                        instructionsString1.SetString("Move the mouse to drag the robot");
                        Vector2 NewPosition = new Vector2(mouseInput.X, mouseInput.Y);
                        robot.SetVirtualPosition(NewPosition);
                        //Update the real position because we are placing it virtually.
                        robot.SetRealPosition(map.GetRealPosition(NewPosition));
                    }
                    // When the mouse is released, refresh the string data 
                    if (lastMouseInput.LeftButton == ButtonState.Pressed && mouseInput.LeftButton == ButtonState.Released)
                    {
                        //Update the instructions that will be visible on screen
                        instructionsString1.SetString("Hold down the left mouse button to drag the robot, right to set direction");
                        instructionsString2.SetString("Hit -S to start the car");
                    }
                    #endregion
                    #region Set Robot Position
                    // Set the robot's direction to face the mouse pointer when the right mouse button is pushed
                    if (mouseInput.RightButton == ButtonState.Pressed && mouseInput.LeftButton == ButtonState.Released)
                    {
                        // Need to get the normalized vector between the mouse pointer and the robot's position
                        Vector2 direction = new Vector2(mouseInput.X - robot.GetVirtualPosition().X, mouseInput.Y - robot.GetVirtualPosition().Y);
                        direction.Normalize();
                        robot.SetDirection(direction);
                    }
                    // When the mouse is released, refresh the string data 
                    if (lastMouseInput.RightButton == ButtonState.Pressed && mouseInput.RightButton == ButtonState.Released)
                    {
                        //Update the instructions that will be visible on screen
                        instructionsString1.SetString("Hold down the left mouse button to drag the robot, right to set direction");
                        instructionsString2.SetString("Hit -S to start the car");
                    }
                    #endregion
                    // Only allow the user to start the simulation when the robot position/direction isn't being changed
                    // If the user strikes the 's' key, change state and start driving!
                    if (mouseInput.LeftButton == ButtonState.Released && mouseInput.RightButton == ButtonState.Released &&
                        keyboardInput.IsKeyDown(Keys.S) && lastKeyboardInput.IsKeyUp(Keys.S))
                    {
                        robot.UpdateMotor((int)MotorID.DRIVE, (int)DriveMotorParams.MAX);
                        currentState = VirtualRobotState.RUN;
                        instructionsString1.SetString("Car is now driving. Press 't' to stop");
                        instructionsString2.SetString("");

                        Message Update = new Message();
                        Update.CreateAlgorithmUpdateMsg(0);
                        communicator.SendMessage(0, Update);
                    }
                    #endregion
                    break;

                case (int)VirtualRobotState.RUN:
                    #region Run the Simulation
                    //If we are running, hitting T will stop the car. Check for this.
                    //Only do this once if the key has been pressed.
                    if (keyboardInput.IsKeyDown(Keys.T) && lastKeyboardInput.IsKeyUp(Keys.T))
                    {
                        //The car has been stopped with the keystroke. Update the instructions and state.
                        //We change the instruction string now so it doesn't do it every frame.
                        instructionsString1.SetString("Car has now stopped. Press 's' to start again");
                        currentState = VirtualRobotState.STOP;

                        robot.UpdateMotor((int)MotorID.DRIVE, (int)DriveMotorParams.STOP);
                    }

                    // First things first. Update the robot.
                    UpdateRobot();

                    // Second, send updates to the server at regular intervals
                    // so that the server does not get flooded.
                    TimeSpan diff = DateTime.Now - updateServerTime;
                    if (diff.Milliseconds > 80)
                    {
                        Message Update = new Message();
                        RobotData data = robot.GetDataCopy();
                        Update.CreateUpdateMessage(0, data.realPosition, data.direction, data.sensors, data.motors, data.velocity);
                        communicator.SendMessage(0, Update);
                        updateServerTime = DateTime.Now;
                    }

                    // Third, get the next message from the server and parse it
                    Message nextMessage;
                    nextMessage = communicator.GetNextMessage();
                    ParseMessage(nextMessage);

                    // If the simulator is ready, grab an update
                    if (requestAlgorithmUpdate == true)
                    {
                        Message AlgRequest = new Message();
                        AlgRequest.CreateAlgorithmUpdateMsg(0);
                        communicator.SendMessage(0, AlgRequest);
                        requestAlgorithmUpdate = false;
                    }
                    #endregion
                    break;

                case (int)VirtualRobotState.STOP:
                    //If the user strikes the 's' key, change state and start driving!
                    if (keyboardInput.IsKeyDown(Keys.S) && lastKeyboardInput.IsKeyUp(Keys.S))
                    {
                        //If we want to continue driving, set motor speed to max and drive
                        robot.UpdateMotor((int)MotorID.DRIVE, (int)DriveMotorParams.MAX);
                        currentState = VirtualRobotState.RUN;
                        instructionsString1.SetString("Car is now driving. Press 't' to stop");

                        // Kick off the messaging structure with an update message
                        Message Update = new Message();
                        Update.CreateAlgorithmUpdateMsg(0);
                        communicator.SendMessage(0, Update);

                    }
                    break;

                default: throw new Exception("Invalid Virtual Robot State");
            }

            // Remember the current input as the input from the previous update
            lastKeyboardInput = keyboardInput;
            lastMouseInput = mouseInput;

            //Update the base
            base.Update(gameTime);
        }
        private void DelayedMessageThread()
        {
            while (true)
            {
                UpdateDelayedMessages();
            }
        }
        private void ParseMessage(Message nextMessage)
        {
            if (nextMessage != null)
            {
                //communicator_log.WriteLog("The next message is " + nextMessage.SaveToString());
                // Here we only care about the Server to Client messages. This is found in message.cs
                // MOTOR_SET,           // Command to set the motor to specific value  Params: {int WhichMotor} {int value}    Usage: Server to Client
                // MOTOR_INC,           // Command to increment the motor setting      Params: {int WhichMotor} {int value}    Usage: Server to Client
                // MOTOR_INVERT,        // Command to invert the current motor setting Params: {int WhichMotor}                Usage: Server to Client
                int WhichMotor = -1;
                int NewSetting = -1;
                int OldSetting = -1;
                switch ((int)nextMessage.GetMessageType())
                {
                    case (int)MessageType.BURST_BEGIN:
                        receiveServerMessages = true;
                        parsedServerMessages = 0;
                        expectedServerMessages = Convert.ToInt32(nextMessage.GetField("count"));
                        break;

                    case (int)MessageType.BURST_END:
                        receiveServerMessages = false;
                        requestAlgorithmUpdate = true;
                        break;

                    case (int)MessageType.SLEEP:
                        try
                        {
                            Thread.Sleep(Convert.ToInt32(nextMessage.GetField("MS")));
                        }
                        catch (FormatException e)
                        {
                            communicator_log.WriteLog("Format Exception: " + e.Message);
                            break;
                        }
                        break;
                    case (int)MessageType.MOTOR_SET:
                        //  Only if we are receiving we should consider this message
                        if (receiveServerMessages == true)
                        {
                            try
                            {
                                int Delay = Convert.ToInt32(nextMessage.GetField("DelayMS"));
                                if (Delay > 0)
                                {
                                    DelayedMessage newDelayedMessage = new DelayedMessage(nextMessage, DateTime.Now, Delay);
                                    lock (delayedMessages)
                                    {
                                        delayedMessages.Add(newDelayedMessage);
                                    }
                                    break;
                                }
                                WhichMotor = Convert.ToInt32(nextMessage.GetField("motor_id"));
                                NewSetting = Convert.ToInt32(nextMessage.GetField("setting"));
                            }
                            catch (FormatException e)
                            {
                                communicator_log.WriteLog("Format Exception: " + e.Message);
                                parsedServerMessages++;
                                break;
                            }
                            robot.UpdateMotor(WhichMotor, NewSetting);
                            parsedServerMessages++;
                        }


                        break;
                    case (int)MessageType.MOTOR_INC:
                        // Message params should be int for which motor and int for setting
                        if (receiveServerMessages == true)
                        {
                            try
                            {
                                int Delay = Convert.ToInt32(nextMessage.GetField("DelayMS"));
                                if (Delay > 0)
                                {
                                    DelayedMessage newDelayedMessage = new DelayedMessage(nextMessage, DateTime.Now, Delay);
                                    lock (delayedMessages)
                                    {
                                        delayedMessages.Add(newDelayedMessage);
                                    }
                                    break;
                                }
                                WhichMotor = Convert.ToInt32(nextMessage.GetField("motor_id"));
                                NewSetting = Convert.ToInt32(nextMessage.GetField("amount"));
                            }
                            catch (FormatException e)
                            {
                                communicator_log.WriteLog("Format Exception: " + e.Message);
                                parsedServerMessages++;
                                break;
                            }
                            // Get the old setting and increment!
                            OldSetting = robot.GetMotorSetting(WhichMotor);
                            robot.UpdateMotor(WhichMotor, OldSetting + NewSetting);

                            parsedServerMessages++;
                        }

                        break;
                    case (int)MessageType.MOTOR_INVERT:
                        // Message params should be int for which motor and int for setting
                        if (receiveServerMessages == true)
                        {
                            try
                            {
                                int Delay = Convert.ToInt32(nextMessage.GetField("DelayMS"));
                                if (Delay > 0)
                                {
                                    DelayedMessage newDelayedMessage = new DelayedMessage(nextMessage, DateTime.Now, Delay);
                                    lock (delayedMessages)
                                    {
                                        delayedMessages.Add(newDelayedMessage);
                                        parsedServerMessages++;
                                    }
                                    break;
                                }
                                WhichMotor = Convert.ToInt32(nextMessage.GetField("motor_id"));
                            }
                            catch (FormatException e)
                            {
                                communicator_log.WriteLog("Format Exception: " + e.Message);
                                parsedServerMessages++;
                                break;
                            }
                            OldSetting = robot.GetMotorSetting(WhichMotor);
                            robot.UpdateMotor(WhichMotor, -OldSetting);
                            parsedServerMessages++;
                        }
                        break;
                    default:
                        break;
                }
            }
        }

        // Return true if we have parsed a new message
        private bool UpdateDelayedMessages()
        {
            // Wait for the completion of the delayed messages (hopefully not too long!)
            for (int i = 0; i < delayedMessages.Count; i++)
            {
                DelayedMessage msg = null;
                lock (delayedMessages)
                {
                     msg = delayedMessages[i];
                }
                TimeSpan Diff = DateTime.Now - msg.GetStartTime();
                if (Diff.Milliseconds > msg.GetDelayPeriod())
                {
                    // Retrieve the next message, and change the delay to 0
                    Message nextMessage = msg.GetMessage();
                    MessageField mf = new MessageField("DelayMS");
                    mf.SetValue("0");
                    nextMessage.SetField(mf);

                    lock (delayedMessages)
                    {
                        delayedMessages.Remove(msg);
                    }

                    ParseMessage(nextMessage);
                    return true;
                }
            }
            return false;
        }
        private void UpdateRobot()
        {
            //Since last update, we have been accelerating at some rate and have been moving at some rate.
            //An approximation to this movement will be to simulate this movement in the following steps:
            //  1. Update the position of the car based on the velocity value from the last frame  
            //  2. Update the sensor readings of the car based on the map.
            //  3. Update the velocity of the car based on the acceleration value from last frame
            //  4. Update the direction of the car based on the directional motor settings
            //  5. Update the acceleration of the car based on the thrust motor settings

            DateTime CurrentTime = DateTime.Now;
            TimeSpan Elapsed = CurrentTime - oldTime;
            oldTime = CurrentTime;

            UpdatePosition(Elapsed.Milliseconds);
            UpdateSensors();
            UpdateDirection(Elapsed.Milliseconds);
            UpdateVelocity(Elapsed.Milliseconds);
        }
        private void UpdatePosition(int ElapsedMS)
        {
            //  1. New Position based on velocity
            //      assuming that the direction vector is unit length, simply multiply it by its X and Y values
            float Velocity = robot.GetVelocity();
            Vector2 Direction = robot.GetDirection();
            Vector2 OldPosition = robot.GetRealPosition();
            int DriveMotor = robot.GetMotorSetting((int)MotorID.DRIVE);
            if (DriveMotor < (int)DriveMotorParams.STOP)
            {
                Direction.X = -Direction.X;
                Direction.Y = -Direction.Y;
            }
            Vector2 Increment = new Vector2((ElapsedMS / 1000f) * Velocity * Direction.X, (ElapsedMS / 1000f) * Velocity * Direction.Y);
            Vector2 NewPosition = OldPosition + Increment;
            robot.SetRealPosition(NewPosition);
            robot.SetVirtualPosition(map.GetVirtualPosition(NewPosition));
        }

        private void UpdateVelocity(int ElapsedMS)
        {
            // Get the Drive motor setting
            int driveMotor = robot.GetMotorSetting((int)MotorID.DRIVE);
            // Calculate the new speed between -0.5 to 0.5
            float speed = Math.Abs((float)(driveMotor - (int)DriveMotorParams.STOP) / ((int)DriveMotorParams.MAX - (int)DriveMotorParams.MIN));
            robot.UpdateVelocity(speed);
        }
        private void UpdateDirection(int ElapsedMS)
        {
            // Get the current steering motor setting
            int steerMotor = robot.GetMotorSetting((int)MotorID.STEERING);
            // Calculate the angle multiplier to use based on the current motor setting
            float multiplier = (float)(steerMotor - (int)SteeringMotorParams.STOP) / ((float)SteeringMotorParams.MAX - (float)SteeringMotorParams.MIN);
            // Set the robot's new driving angle

            int driveMotor = robot.GetMotorSetting((int)MotorID.DRIVE);
            if (driveMotor < (int)DriveMotorParams.STOP)
                robot.IncAngle(-multiplier * (float)Math.PI / 30.0f);
            else
                robot.IncAngle(multiplier * (float)Math.PI / 30.0f);
        }

        private void UpdateSensors()
        {
            //Need a variable to hold how many blocks are uncovered by the sensor range
            int NumBlocks = 0;
            Block[] SensorData;

            //Go through all the sensors, uncovering the blocks within their range and checking if there are obstacles
            for (int i = 0; i < (int)SensorID.NUM_SENSORS; i++)
            {
                float TempSensor = robot.GetSensorReading(i);

                Vector2 RobotDir = robot.GetDirection();

                // Make a copy of the robot direction
                Vector2 Dir = new Vector2(RobotDir.X,RobotDir.Y);
                Vector2 NewDir;
                double angle = 0;
                switch(i)
                {
                    case (int)SensorID.LEFT:
                        NewDir = new Vector2(Dir.X, Dir.Y);
                        angle = -Math.PI / 2.3;
                        NewDir.X = (float)Math.Cos(angle) * Dir.X - (float)Math.Sin(angle) * Dir.Y;
                        NewDir.Y = (float)Math.Sin(angle) * Dir.X + (float)Math.Cos(angle) * Dir.Y;
                        Dir = NewDir;
                        break;
                    case (int)SensorID.RIGHT:
                        NewDir = new Vector2(Dir.X, Dir.Y);
                        angle = Math.PI / 2.3;
                        NewDir.X = (float)Math.Cos(angle) * Dir.X - (float)Math.Sin(angle) * Dir.Y;
                        NewDir.Y = (float)Math.Sin(angle) * Dir.X + (float)Math.Cos(angle) * Dir.Y;
                        Dir = NewDir;
                        break;
                        
                    default:
                        break;
                }

                //Get the blocks that are covered by the sensor.
                SensorData = map.GetLineOfBlocks(robot.GetRealPosition(), (float)SensorParams.RANGE, Dir, ref NumBlocks);
                //Run through the blocks and see if there are any obstacles
                bool NoObstacle = true;
                for (int j = 0; j < NumBlocks; j++)
                {
                    //If the block is considered blocked, then update the sensor reading. We only need the first
                    // reading, everything behind this block should be 'blocked'
                    if (SensorData[j].CurrentState == BlockState.BLOCKED)
                    {
                        TempSensor = map.GetRealDistance(robot.GetVirtualPosition(), SensorData[j].BlockCenter);
                        NoObstacle = false;
                        break;
                    }
                }
                if (NoObstacle==true)
                {
                    TempSensor = (float)SensorParams.MAX;
                }
                robot.UpdateSensor(i, TempSensor);
            }
        }

        /// <summary>
        /// This is called when the game should draw itself.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Draw(GameTime gameTime)
        {
            //Clear the device
            graphics.GraphicsDevice.Clear(Color.CornflowerBlue);

            // TODO: Add your drawing code here
            map.Draw();

            instructionsString1.Draw();
            instructionsString2.Draw();

            // If the robot is on the map, then draw it.
            if (map.CheckBoundaries(robot.position,false) == true)
            {
                robot.Draw();
            }
            base.Draw(gameTime);
        }
    }
}
